#include "../src/bogackishampine.hh"
#include "../src/pianovec.hh"

#include <cassert> 
#include <iostream> 

namespace
{
  using namespace playerpiano;

  bool
  fuzzy_eq (float a,
            float b)
    {
      return fabs (a - b) <= 1e-3 * (1.0 + fabs (a) + fabs (b));
    }

  struct PowerLaw
    {
      float alpha;
      mutable unsigned int evals;

      PowerLaw (float _alpha) : alpha (_alpha), evals (0) { }

      float
      operator() (float x,
                  float) const
        {
          return ++evals, powf (x, alpha);
        }
    };

  void
  test_bogacki_shampine_once ()
    {
      float alpha;

      do
        {
          alpha = 2.0 * drand48 () - 1.0;
        }
      while (fabsf (alpha) < 0.05);

      float x0;

      do
        {
          x0 = 2.0 * drand48 ();
        }
      while (x0 < 0.05);

      float y0 = 2.0 * drand48 () - 1.0;
      float x1 = x0 + 2.0 * drand48 ();
      PowerLaw power_law (alpha);

      std::pair<bool, float> integral = 
        bogacki_shampine_2_3 (power_law,
                              x0,
                              y0,
                              x1,
                              1e-4,
                              0.0001,
                              1.0,
                              1000);

      float plusone = 1.0f + alpha;
      float y1 = y0 + (powf (x1, plusone) - powf (x0, plusone)) / plusone;

      assert (integral.first || 
              (std::cerr << "integral.first = false"
                         << " x0 = " << x0
                         << " y0 = " << y0
                         << " x1 = " << x1
                         << " y1 = " << y1
                         << " alpha = " << alpha
                         << " evals = " << power_law.evals
                         << std::endl,
              0));

      assert (power_law.evals <= 3 * ceilf (1000.0f / 3.0f)
              || (std::cerr << "power_law.evals = " 
                            << power_law.evals 
                            << std::endl,
                  0));

      assert (fuzzy_eq (integral.second, y1) || (
              std::cerr << "integral.second = " << integral.second
                        << " x0 = " << x0
                        << " y0 = " << y0
                        << " x1 = " << x1
                        << " y1 = " << y1
                        << " alpha = " << alpha
                        << " evals = " << power_law.evals
                        << std::endl,
              0));
    }

  struct Cosine
    {
      float theta;
      mutable unsigned int evals;

      Cosine (float _theta) : theta (_theta), evals (0) { }

      float
      operator() (float x,
                  float) const
        {
          return ++evals, cosf (theta * x);
        }
    };

  void
  test_bogacki_shampine_mega_once ()
    {
      float theta;

      do
        {
          theta = 2.0 * drand48 () - 1.0;
        }
      while (fabsf (theta) < 0.01);

      float x0 = 2.0 * drand48 () - 1.0;
      float y0 = 2.0 * drand48 () - 1.0;
      float x1 = x0 + 2.0 * drand48 ();
      Cosine cosine (theta);

      std::pair<bool, float> integral = 
        bogacki_shampine_2_3 (cosine,
                              x0,
                              y0,
                              x1,
                              1e-4,
                              0.001,
                              1.0,
                              1000);

      float y1 = y0 + (sinf (theta * x1) - sinf (theta * x0)) / theta;

      assert (integral.first || 
              (std::cerr << "integral.first = false"
                         << " x0 = " << x0
                         << " y0 = " << y0
                         << " x1 = " << x1
                         << " y1 = " << y1
                         << " theta = " << theta
                         << " evals = " << cosine.evals
                         << std::endl,
              0));

      assert (cosine.evals <= 3 * ceilf (1000.0f / 3.0f)
              || (std::cerr << "cosine.evals = " 
                            << cosine.evals 
                            << std::endl,
                  0));

      assert (fuzzy_eq (integral.second, y1) || (
              std::cerr << "integral.second = " << integral.second
                        << " x0 = " << x0
                        << " y0 = " << y0
                        << " x1 = " << x1
                        << " y1 = " << y1
                        << " theta = " << theta
                        << " evals = " << cosine.evals
                        << std::endl,
              0));
    }

  struct CircularMotion
    {
      float theta;
      mutable unsigned int evals;

      CircularMotion (float _theta) : theta (_theta), evals (0) { }

      template<typename Expr>
      PianoVec
      operator() (float,
                  const Expr& v) const
        {
          PianoVec rv (2, 0);

          rv[0] = v[1];
          rv[1] = -theta * v[0];

          return ++evals, rv;
        }
    };

  unsigned int 
  test_bogacki_shampine_ultra_once ()
    {
      float theta;

      do
        {
          theta = 2.0 * drand48 ();
        }
      while (fabsf (theta) < 0.01);

      PianoVec y0 (2);
      y0[0] = 2.0 * drand48 () - 1.0;
      y0[1] = 0;

      float x0 = 2.0 * drand48 () - 1.0;
      float x1 = x0 + 2.0 * drand48 ();
      CircularMotion circular (theta);

      std::pair<bool, PianoVec> integral = 
        bogacki_shampine_2_3 (circular,
                              x0,
                              y0,
                              x1,
                              1e-4,
                              0.001,
                              1.0,
                              1000);

      float y1 = y0[0] * cos ((x1 - x0) * sqrt (theta));

      assert (integral.first || 
              (std::cerr << "integral.first = false"
                         << " x0 = " << x0
                         << " y0 = " << y0[0]
                         << " x1 = " << x1
                         << " y1 = " << y1
                         << " theta = " << theta
                         << " evals = " << circular.evals
                         << std::endl,
              0));

      assert (circular.evals <= 3 * ceilf (1000.0f / 3.0f)
              || (std::cerr << "circular.evals = " 
                            << circular.evals 
                            << std::endl,
                  0));

      assert (fuzzy_eq (integral.second[0], y1) || (
              std::cerr << "integral.second = " << integral.second[0]
                        << " x0 = " << x0
                        << " y0 = " << y0[0]
                        << " x1 = " << x1
                        << " y1 = " << y1
                        << " theta = " << theta
                        << " evals = " << circular.evals
                        << std::endl,
              0));

      return circular.evals;
    }

  void
  test_bogacki_shampine ()
    {
      unsigned int evals = 0;

      for (unsigned int n = 0; n < 1000; ++n)
        {
          test_bogacki_shampine_once ();
          test_bogacki_shampine_mega_once ();
          evals += test_bogacki_shampine_ultra_once ();
        }

//      std::cerr << "evals = " << evals << std::endl;
    }
}

int
main (void)
{
  srand48 (69);
  test_bogacki_shampine ();

  return 0;
}


